import math as m

import cv2
import matplotlib.pyplot as plt
import numpy as np
from rich import print

from tma.utils.custom_types import *
from tma.utils.prediction import *

"""
@TODO todo list
1. detect alnormal bearing lines as early as possible
2. give Accuracy figure of estimation
3. target heading
4. detect target heading from image
5. thoroughly test case for self.normalized_data (x1 < 0 or y1 <0)

@NOTE
1. image process step:
   original_lines -> switch_coordinate_lines -> normalized_data -> merge_similar_line
                                                                     │
                                                                     ▼
                                                                 self.lines
2. color picker
    https://www.w3schools.com/colors/colors_converter.asp

3. opencv detect arrow
    https://www.cnpython.com/qa/1312061
    https://www.php1.cn/detail/ShiYong_opencv_P_690a360d.html
    https://docs.opencv.org/4.x/d1/dee/tutorial_introduction_to_pca.html
    https://automaticaddison.com/how-to-determine-the-orientation-of-an-object-using-opencv/

4. opencv color mask
    https://www.delftstack.com/howto/python/opencv-inrange/#google_vignette

5. opencv resize
    https://www.geeksforgeeks.org/image-resizing-using-opencv-python/
    https://learnopencv.com/image-resizing-with-opencv/

"""


def plot_line_through_points(line: t_line = None, p1: t_point = None, p2: t_point = None):
    """plot a line"""
    if line:
        x1, y1, x2, y2 = line
    else:
        x1, y1, x2, y2 = list(p1 + p2)
    x = [x1, x2]
    m = (y2 - y1) / (x2 - x1)
    c = y1 - (m * x1)
    y = [m * xi + c for xi in x]

    ax.plot(x, y)


if __name__ == "__main__":
    # IMAGE_FILE = "D:/temp_d/t6.png"
    # IMAGE_FILE = "D:/temp_d/t5.png"
    # IMAGE_FILE = "D:/temp_d/d2/p3.png"
    #
    IMAGE_FILE = "D:/temp_d/data01/s4/p7.png"
    # IMAGE_FILE = "D:/temp_d/data01/s3/p5.png"
    # IMAGE_FILE = "D:/temp_d/data01/s2/p2.png"
    # IMAGE_FILE = "D:/temp_d/data01/s1/p2.png"
    #
    # IMAGE_FILE = "D:/temp_d/data02/s2/p4.png"
    # IMAGE_FILE = "D:/temp_d/data02/s3/p4.png"
    # IMAGE_FILE = "D:/temp_d/data02/s4/p4.png"
    # IMAGE_FILE = "D:/temp_d/data02/s5/p4.png"
    #
    # IMAGE_FILE = "D:/temp_d/data03/p10.png"
    # IMAGE_FILE = "D:/temp_d/data03/p1.png"
    #
    # IMAGE_FILE = "D:/temp_d/d4/p7.png"
    # IMAGE_FILE = "D:/temp_d/data01/s4/p8.png"  # two leg track
    # "D:/temp_d/test-data/d4/p4.png" # unable to detect bearing line
    # IMAGE_FILE = "D:/temp_d/d5/s15.png"
    # !!@ best match
    # IMAGE_FILE = "D:/temp/data01/s1/p5.png"

    tma = TMA(Target(), IMAGE_FILE)

    #
    # tma.predict_with_target_track()
    tma.predict_with_target_track_new()
    # tma.predict_with_target_track_new_05()
    #
    # tma.predict_without_target_track()
    tma.predict_without_target_track_new_02()
    # tma.predict_without_target_track_new_05()
    # tma.predict_without_target_track_full_brute_force()
    #
    tma.single_leg_ekelund_ranging()

    print("*" * 55)
    print("tma.lines")
    print(tma.lines)
    # print(f"target view_center_cross_distance: {tma.view_center_cross_distance}")
    if tma.target.pd_with_track.track_detected:
        print("-------------------------------------------- pd_with_track ---")
        print(f"number of bearing line: {len(tma.lines)}")
        print(f"target track: {tma.target.pd_with_track.track}")
        print("target track smallest_fitness_error:" f" {tma.target.pd_with_track.track_error}")
        print(f"target track travel_distance: {tma.target.pd_with_track.track_distance}")
        print(f"target track heading: {tma.target.pd_with_track.track_heading}")
        # print(tma.target.pd_with_track.top3_track)
    print("--------------------------------------------- pd_without_track ---")
    print(f"number of bearing line: {len(tma.lines)}")
    print(f"target track: {tma.target.pd_without_track.track}")
    print("target track smallest_fitness_error:" f" {tma.target.pd_without_track.track_error}")
    print(f"target track travel_distance: {tma.target.pd_without_track.track_distance}")
    print(f"target track heading: {tma.target.pd_without_track.track_heading}")
    # print(tma.target.pd_without_track.top3_track)
    print("target bearing line change ratio")
    print(tma.bl_change_ratio)
    # print("ekelund_range")
    # print(tma.target.ekelund_range)
    print("+" * 55)

    # !!! plot
    fig, ax = plt.subplots()
    ax.set_aspect("equal")

    for line in tma.lines:
        plot_line_through_points(line)
    plot_line_through_points(tma.view_center_group_cross_line)
    ax.plot(
        [tma.lines[0][0], tma.lines[0][2]],
        [tma.lines[0][1], tma.lines[0][3]],
        "--",
        label="line-0",
        c="r",
    )
    ax.plot(
        [tma.lines[-1][0], tma.lines[-1][2]],
        [tma.lines[-1][1], tma.lines[-1][3]],
        "--",
        label=f"line-{len(tma.lines)}",
        c="b",
    )

    ax.scatter(*tma.view_center, c="y", s=200, zorder=5)

    if tma.target.pd_with_track.track_detected:
        cross_points = [find_intersection_point(i, tma.target.pd_with_track.track) for i in tma.lines]
        for p in cross_points:
            ax.scatter(*p, c="#98c379")

        x1, y1 = cross_points[0]
        x2, y2 = cross_points[-1]
        ax.plot((x1, x2), (y1, y2), label="detected_track", c="#98c379")

    cross_points = [find_intersection_point(i, tma.target.pd_without_track.track) for i in tma.lines]
    for p in cross_points:
        ax.scatter(*p, c="#b078dd")

    x1, y1 = cross_points[0]
    x2, y2 = cross_points[-1]
    ax.plot((x1, x2), (y1, y2), label="estimated_track", c="#b078dd")

    lines = [
        # [257.5303830485259, 486.4358021360041, 445.6330028572648, 596.7361622604452],
    ]
    for p in lines:
        plot_line_through_points(p)

    points = []

    for p in points:
        plt.scatter(*p)

    ax.legend()
    plt.show()
